#ifndef __ASTAR_H__
#define __ASTAR_H__

#include <map>
#include <memory>
#include "NodeVector3.hpp"
#include "AStarWorldVector3.h"
#include "AStarStorageVector3.hpp"

using namespace PocketSurvival::Engine;

namespace PocketSurvival
{

    class AStarVector3
    {
    private:
        int32_t m_nodesCacheIndex;

        std::vector<std::shared_ptr<NodeVector3>> m_nodesCache;

        std::vector<std::shared_ptr<NodeVector3>> m_openHeap;

        std::vector<Vector3> m_neighbors;

    public:
        float PathCost;
        std::vector<Vector3> *Path;
        AStarWorldVector3 World;
        AStarStorageVector3 OpenStorage;
        AStarStorageVector3 ClosedStorage;

    public:
        void buildPathFromEndNode(std::shared_ptr<NodeVector3> startNode, std::shared_ptr<NodeVector3> endNode);

        void findPath(const Vector3 &start, const Vector3 &end, float minHeuristic, int32_t maxPositionsToCheck);

        void heapEnqueue(std::shared_ptr<NodeVector3> node);

        std::shared_ptr<NodeVector3> heapDequeue();

        void heapUpdate(std::shared_ptr<NodeVector3> node);

        void heapifyFromPosToEnd(int32_t pos);

        void heapifyFromPosToStart(int32_t pos);

        std::shared_ptr<NodeVector3> newNode(const Vector3 &position, const Vector3 &previousPosition, float g, float h);

    };
}

#endif
